/*
 * Main thread, only used to launch the various tasks
 */

# include <init_systems.h>
# include <log.h>
# include <time_tools.h>
# include "robo_control.h"
# include "main_controller.h"
# include "buttons_reader.h"

//130.237.226.221

using namespace kthrobot;
using namespace std;
using namespace Group5;

int main (int argc, char **argv) {
  bool cont = 1;

  //while(cont){
    Init::RegisterCommon();
    RRCon::Register();
    if ( !Init::Systems(argc, argv) ) return 1;


    {
      bool *button_pressed;
      ButtonsReader buttons_reader;
      buttons_reader.Start();

      // Waiting for a press on the center button to start
      button_pressed = buttons_reader.WaitForButton();

      Main_Controller robot;
      if(button_pressed[ButtonsReader::CENTER_BUTTON]){
        robot.SetMode(Main_Controller::EXPLORING);
        Log::Write("Mode set to EXPLORING");
      }
      else if(button_pressed[ButtonsReader::RIGHT_BUTTON]){
        robot.SetMode(Main_Controller::BALLCATCHING);
        Log::Write("Mode set to BALLCATCHING");
      }
      //give it sometime...
      TimeTools::WaitMS(1040);
      robot.Start();

      // Waiting for a press on the center button to stop
      button_pressed = buttons_reader.WaitForButton();

      if(button_pressed[ButtonsReader::LEFT_BUTTON]){
        cont = false;
      }
      Init::SignalShutdown(); // Asks all taksk to terminate
    }

    Init::Shutdown();
//  }
  return 0;
}
